#include "qdec.h"
#include "same51n20a.h"

#define DIRECTION_NONE  0u
#define DIRECTION_UP    1u
#define DIRECTION_DOWN  2u

#define READ_A_LEVEL (PORT_REGS->GROUP[2].PORT_IN & 0x10000) ? 1 : 0
#define READ_B_LEVEL (PORT_REGS->GROUP[2].PORT_IN & 0x20000) ? 1 : 0

static uint8 _currentState = 0;
static uint8 _currentDirection = 0;

static sint16 dec_count = 0;

#define QDEC_INTERRUPT 0u
#define QDEC_PERIPHERAL 1u
#define QDEC_MODE QDEC_PERIPHERAL
// PC16 17
static inline void qdec_gpio_init(void) {
	// 设置I/O 输入，外部中断复用
#if (QDEC_MODE == QDEC_INTERRUPT)
	MCLK_REGS->MCLK_APBBMASK |= MCLK_APBBMASK_PORT(1);
	PORT_REGS->GROUP[2].PORT_DIRCLR |= PORT_PC16 | PORT_PC17;
	PORT_REGS->GROUP[2].PORT_OUTSET |= PORT_PC16 | PORT_PC17;
	PORT_REGS->GROUP[2].PORT_WRCONFIG =
	PORT_WRCONFIG_HWSEL(1) |    // High 16pin
	PORT_WRCONFIG_WRPINCFG(1) |
	PORT_WRCONFIG_WRPMUX(0) |   
	PORT_WRCONFIG_PMUX(1) |    /* function A */
	PORT_WRCONFIG_INEN(1) |
	PORT_WRCONFIG_PULLEN(1) |
	PORT_WRCONFIG_PINMASK(0x0003) | /* PC16 PC17 */
	PORT_WRCONFIG_PMUXEN(1);
#endif
	// 设置I/O 输入，正交解码复用
#if (QDEC_MODE == QDEC_PERIPHERAL)
	MCLK_REGS->MCLK_APBBMASK |= MCLK_APBBMASK_PORT(1);
	PORT_REGS->GROUP[2].PORT_DIRCLR |= PORT_PC16 | PORT_PC17;
	PORT_REGS->GROUP[2].PORT_OUTSET |= PORT_PC16 | PORT_PC17;
	PORT_REGS->GROUP[2].PORT_WRCONFIG =
	PORT_WRCONFIG_HWSEL(1) |    // High 16pin
	PORT_WRCONFIG_WRPINCFG(1) |
	PORT_WRCONFIG_WRPMUX(1) |   
	PORT_WRCONFIG_PMUX(6) |    /* function G */
	PORT_WRCONFIG_INEN(1) |
	PORT_WRCONFIG_PULLEN(1) |
	PORT_WRCONFIG_PINMASK(0x0003) | /* PC16 PC17 */
	PORT_WRCONFIG_PMUXEN(1);
#endif
}

static inline void qdec_peripheral_init(void) {
#if (QDEC_MODE == QDEC_INTERRUPT)
	MCLK_REGS->MCLK_APBAMASK |= MCLK_APBAMASK_EIC(1);
	GCLK_REGS->GCLK_PCHCTRL[EIC_GCLK_ID] = GCLK_PCHCTRL_CHEN(1) | GCLK_PCHCTRL_GEN_GCLK0;
	EIC_REGS->EIC_CONFIG[0] |= EIC_CONFIG_FILTEN0(1) | EIC_CONFIG_FILTEN1(1) | EIC_CONFIG_SENSE0_BOTH | EIC_CONFIG_SENSE1_BOTH;
	EIC_REGS->EIC_INTENSET |= 0x3; // (PORT_PC16 >> 16) | (PORT_PC17 >> 16);
	EIC_REGS->EIC_DPRESCALER |= EIC_DPRESCALER_STATES0_LFREQ7 | EIC_DPRESCALER_PRESCALER1_DIV4;
	EIC_REGS->EIC_DEBOUNCEN |= EIC_DEBOUNCEN_DEBOUNCEN(0x3);
	EIC_REGS->EIC_CTRLA |= EIC_CTRLA_CKSEL_CLK_GCLK | EIC_CTRLA_ENABLE(1);

	NVIC_EnableIRQ(EIC_EXTINT_0_IRQn);
	NVIC_EnableIRQ(EIC_EXTINT_1_IRQn);
#endif
#if (QDEC_MODE == QDEC_PERIPHERAL)
	MCLK_REGS->MCLK_APBCMASK |= MCLK_APBCMASK_PDEC(1);
	GCLK_REGS->GCLK_PCHCTRL[PDEC_GCLK_ID] = GCLK_PCHCTRL_CHEN(1) | GCLK_PCHCTRL_GEN_GCLK0;
	PDEC_REGS->PDEC_CTRLA |= PDEC_CTRLA_PINEN0(1) | PDEC_CTRLA_PINEN1(1) | PDEC_CTRLA_CONF_X4 | PDEC_CTRLA_ANGULAR(0x7);
    PDEC_REGS->PDEC_FILTER = 0x80;
	PDEC_REGS->PDEC_CTRLA |= PDEC_CTRLA_ENABLE(1);
	while(1 == (PDEC_REGS->PDEC_SYNCBUSY & PDEC_SYNCBUSY_ENABLE(1)));
	PDEC_REGS->PDEC_CTRLBSET |= PDEC_CTRLBSET_CMD_START;
#endif
}

void qdec_init(void) {
    qdec_gpio_init();
    qdec_peripheral_init();
#if (QDEC_MODE == QDEC_INTERRUPT)
    uint8 init_ab = (READ_A_LEVEL << 1) | READ_B_LEVEL;
    switch(init_ab) {
        case 0: // A=0 B=0
            _currentState = 1;
        break;
        case 1: // A=0 B=1
            _currentState = 2;
        break;
        case 3: // A=1 B=1
            _currentState = 3;
        break;
        case 2: // A=1 B=0
            _currentState = 4;
        break;
        default:
        break;
    }
#endif
}

sint16 qdec_get_count(void) {
#if (QDEC_MODE == QDEC_INTERRUPT)
    return (sint16) dec_count;
#endif

#if (QDEC_MODE == QDEC_PERIPHERAL)
    PDEC_REGS->PDEC_CTRLBSET |= PDEC_CTRLBSET_CMD_READSYNC;
    return (sint16) PDEC_REGS->PDEC_COUNT;
#endif
}

void qdec_int_service(void) {
    if(_currentState < 1 || _currentState > 4) return;
    uint8 next_ab = (0x30000 & PORT_REGS->GROUP[2].PORT_IN) >> 16;
    // 四个状态，四个分支判断
    switch(_currentState) {
        case 1: // A=0 B=0
            if(next_ab == 0x1) {
                _currentDirection = DIRECTION_UP;
                _currentState = 2;
            } else if(next_ab == 0x2) {
                _currentDirection = DIRECTION_DOWN;
                _currentState = 4;
            } else {
                _currentDirection = DIRECTION_NONE;
            }
        break;
        case 2: // A=0 B=1
            if(next_ab == 0x3) {
                _currentDirection = DIRECTION_UP;
                _currentState = 3;
            } else if(next_ab == 0x0) {
                _currentDirection = DIRECTION_DOWN;
                _currentState = 1;
            } else {
                _currentDirection = DIRECTION_NONE;
            }
        break;
        case 3: // A=1 B=1
            if(next_ab == 0x2) {
                _currentDirection = DIRECTION_UP;
                _currentState = 4;
            } else if(next_ab == 0x1) {
                _currentDirection = DIRECTION_DOWN;
                _currentState = 2;
            } else {
                _currentDirection = DIRECTION_NONE;
            }
        break;
        case 4: // A=1 B=0
            if(next_ab == 0x0) {
                _currentDirection = DIRECTION_UP;
                _currentState = 1;
            } else if(next_ab == 0x3) {
                _currentDirection = DIRECTION_DOWN;
                _currentState = 3;
            } else {
                _currentDirection = DIRECTION_NONE;
            }
        break;
        default:
        break;
    }

    if(_currentDirection == DIRECTION_UP) {
        // 正向计数增加 
        dec_count++;
    } else if(_currentDirection == DIRECTION_DOWN) {
        // 反向计数减少
        dec_count--;
    }
}
